//这个为用pcl设置大的欧式聚类半径阈值，先将所有lagori先全部聚类分割
//出来，然后再对聚类出来的点云进行多次圆柱拟合，直到找不到圆柱为止。

#include "LdPcl.h"
int main()
{
    pcl::PointCloud<pcl::PointXYZINormal>::Ptr test(new pcl::PointCloud<pcl::PointXYZINormal>());
    pcl::console::TicToc tt;
    std::vector<std::string> BroadCode;
    BroadCode.push_back("000000000000001");
    LdsLidar &mylidar = LdsLidar::GetInstance();
    bool init = mylidar.InitLdsLidar(BroadCode, 100000);
    sleep(1);
    while (!init)
    {
        init = mylidar.InitLdsLidar(BroadCode, 100000);
        sleep(1);
    }
    char key;

    while (key != 'c')
    {
        // std::string saved_path = "/home/milo/Documents/visual/2Champion/PCL/data/Conditional/1_1000.pcd";
        // Pcloud OriginCloud(new Cloud());
        // OriginCloud = mylidar.GetPointCloud(1);
        PcloudI OriginCloud(new CloudI());
        // pcl::io::loadPCDFile(saved_path, *OriginCloud);

        tt.tic();
        // OriginCloud = mylidar.GetPointCloud_I();
        mylidar.getcloud(OriginCloud);
        // sleep(1);
        std::cerr << "Loading...\n";
        int count = 0;
        while (OriginCloud == nullptr || OriginCloud->size() < 1000)
        {
            sleep(0.01);
            // OriginCloud = mylidar.GetPointCloud();
            // OriginCloud = mylidar.GetPointCloud_I();
            mylidar.getcloud(OriginCloud);
            count++;
        }
        std::cout << "Count is " << count << std::endl;
        std::cout << "The time of getting the cloud is : " << tt.toc() << " ms" << std::endl;
        std::cout << "The points of the cloud is : " << OriginCloud->size() << std::endl;
        pcl::visualization::CloudViewer origin("Origin Cloud");
        origin.showCloud(OriginCloud);
        while (!origin.wasStopped())
        {
        }

        // std::string pcd_saved = "/home/milo/Documents/visual/2Champion/PCL/data/3_3000.pcd";
        // pcl::io::loadPCDFile(pcd_saved, *OriginCloud);
        PcloudI Fil_Cloud(new CloudI());
        Filter(OriginCloud, Fil_Cloud);
        // pcl::visualization::CloudViewer V_Filter("V_Filter");
        // V_Filter.showCloud(Fil_Cloud);
        // while (!V_Filter.wasStopped())
        // {
        // }

        // Pcnormal normals(new Cnormal());
        Pfcloud cloud_with_normals(new Fcloud());
        ComputeNormals(Fil_Cloud, cloud_with_normals);
        // RemovePlanes(Fil_Cloud, cloud_with_normals, 5, true);
        std::vector<pcl::PointIndices> Indices;
        // Eucluster(Fil_Cloud, Indices);
        ConditionalEucluster(cloud_with_normals, Indices, true);
        std::cout << "the size of Fil_cloud is: " << Fil_Cloud->size() << std::endl;
        std::cout << "the size of the cloud_with_noramls is :" << cloud_with_normals->size() << std::endl;
        for (std::vector<pcl::PointIndices>::const_iterator it = Indices.begin();
             it != Indices.end();
             it++)
        {
            PcloudI Cy_cloud(new CloudI());
            Pcnormal Cy_normal(new Cnormal());
            std::cout << "the indices size is: " << it->indices.size() << std::endl;
            // Cy_normal->resize(it->indices.size());
            // Pcloud temp(new Cloud());
            for (const auto &idx : it->indices)
            {
                Cy_cloud->push_back((*Fil_Cloud)[idx]);
                // Cy_normal->push_back((*cloud_with_normals)[idx].normal);
                // Cy_normal->points.at(idx).normal_x = (*cloud_with_normals)[idx].normal_x;
                // Cy_normal->points.at(idx).normal_y = (*cloud_with_normals)[idx].normal_y;
                // Cy_normal->points.at(idx).normal_z = (*cloud_with_normals)[idx].normal_z;
                // Cy_normal->points.at(idx).curvature = (*cloud_with_normals)[idx].curvature;
                pcl::Normal temp;
                // temp.normal = (*cloud_with_normals)[idx].normal;
                memcpy(temp.normal, (*cloud_with_normals)[idx].normal, 3 * sizeof(float));
                Cy_normal->push_back(temp);
            }

            Cy_cloud->width = Cy_cloud->size();
            Cy_cloud->height = 1;
            Cy_cloud->is_dense = false;
            bool result = true;
            // pcl::visualization::CloudViewer After_Eu("Eucluster");
            // After_Eu.showCloud(Cy_cloud);
            // while (!After_Eu.wasStopped())
            // {
            // }

            std::vector<float> radius;

            while (true)
            {
                std::cout << "the size of the Cy_cloud and Cy_normals are: " << Cy_cloud->size() << "   " << Cy_normal->size() << std::endl;
                PcloudI CyExtraction(new CloudI());
                Pcnormal NorExtraction(new Cnormal());
                pcl::ModelCoefficients::Ptr coe(new pcl::ModelCoefficients());
                result = FitCylinder(Cy_cloud, Cy_normal, CyExtraction, NorExtraction, coe, false);
                if (!result)
                    break;

                float r = coe->values[6];
                if (radius.size() >= 0)
                {
                    bool draw = true;
                    for (size_t i = 0; i < radius.size(); i++)
                    {
                        if (abs(r - radius[i]) / radius[i] <= 0.15)
                        {
                            draw = false;
                            break;
                        }
                    }
                    if (draw)
                    {
                        radius.push_back(r);
                        std::cout << "The num of Cylinder Points :" << CyExtraction->size() << std::endl;
                        AxialDrawing(OriginCloud, coe);
                        // pcl::visualization::CloudViewer Final("Draw Cylinder Axial");
                        // Final.showCloud(OriginCloud);
                        // while (!Final.wasStopped())
                        // {
                        // }
                    }
                    // RemovePlanes(CyExtraction, NorExtraction, 1, true);
                    if (CyExtraction->size() < 20)
                    {
                        break;
                    }
                    *Cy_cloud = *CyExtraction;
                    *Cy_normal = *NorExtraction;
                }
            }
        }
        // pcl::visualization::CloudViewer AllFinal("All All All Axial");
        // AllFinal.showCloud(OriginCloud);
        // while (!AllFinal.wasStopped())
        // {
        // }
        key = cv::waitKey(1);
    }
}